
import lejos.robotics.subsumption.*;
import lejos.nxt.*;

/**
 * Demonstration of the Behavior subsumption classes.
 * 
 * Requires a wheeled vehicle with two independently controlled
 * motors connected to motor ports A and C, and 
 * a touchmsensor connected to sensor  port 1 and
 * an ultrasonic sensor connected to port 3;
 * 
 * @author Brian Bagnall and Lawrie Griffiths, modified by Roger Glassey
 *
 */
public class BumperCar
{

	public static void main(String[] args)
	{
		Button.LEFT.addButtonListener(new ButtonListener() {

			@Override
			public void buttonReleased(Button arg0) {
				// TODO Auto-generated method stub

			}

			@Override
			public void buttonPressed(Button arg0) {
				System.exit(0);

			}
		});

		Motor.A.setSpeed(400);
		Motor.C.setSpeed(400);
		Sampler samp = new Sampler();
		samp.setDaemon(true);
		samp.start();
		Behavior b1 = new DriveForward();
		Behavior b2 = new DetectWall(samp);
		Behavior b3 = new Exit();
		Behavior[] behaviorList =
		{
				b3, b1, b2
		};
		Arbitrator arbitrator = new Arbitrator(behaviorList);
		LCD.drawString("Bumper Car",0,1);
		Button.waitForPress();
		arbitrator.start();
	}
}

class DriveForward implements Behavior
{

	private boolean _suppressed = false;

	public boolean takeControl()
	{
		return true;  // this behavior always wants control.
	}

	public void suppress()
	{
		_suppressed = true;// standard practice for suppress methods
	}

	public void action()
	{
		_suppressed = false;
		Motor.A.forward();
		Motor.C.forward();
		while (!_suppressed)
		{
			Thread.yield(); //don't exit till suppressed
		}
		Motor.A.stop(); // not strictly necessary, but good probramming practice
		Motor.C.stop();
	}
}


class DetectWall implements Behavior
{
	private Sampler samp;

	public DetectWall(Sampler samp)
	{
		this.samp = samp;
	}

	public boolean takeControl()
	{
		return samp.TouchIsPressed() || samp.getDistance() < 25;
	}

	public void suppress()
	{
		//Since  this is highest priority behavior, suppress will never be called.
	}

	public void action()
	{
		boolean pressed = true;
		while(pressed)
		{
			pressed = false;
			Motor.A.backward();
			Motor.C.backward();
			if(samp.TouchIsPressed())
			{
				pressed = true;
				continue;
			}

			try {
				Thread.sleep(1000);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
			}
			if(samp.TouchIsPressed())
			{
				pressed = true;
				continue;
			}

			Motor.A.rotate(-180, true);// start Motor.A rotating backward
			Motor.C.rotate(-360, true);  // rotate C farther to make the turn
			while(Motor.C.isMoving())
			{
				if(samp.TouchIsPressed())
				{
					pressed = true;
					continue;
				}				
			}
		}
	}
}

class Exit implements Behavior
{

	@Override
	public void action() {
		System.exit(0);
	}

	@Override
	public void suppress() {
		//Aldrig supress		
	}

	@Override
	public boolean takeControl() {
		return Button.ESCAPE.isPressed();
	}

}

class Sampler extends Thread
{
	private TouchSensor touch1;
	private TouchSensor touch2;
	private UltrasonicSensor sonar;
	private int distance = 0;

	public Sampler()
	{
		touch1 = new TouchSensor(SensorPort.S1);
		touch2 = new TouchSensor(SensorPort.S2);
		sonar = new UltrasonicSensor(SensorPort.S3);		
	}

	public void run()
	{	
		while(true)
		{
			sonar.ping();
			Sound.pause(20);
			distance = sonar.getDistance();
			LCD.drawString("Distance " + distance, 0, 0);
			LCD.refresh();

			try {
				sleep(200);
			} catch (InterruptedException e) {
			}			
		}
	}

	public boolean TouchIsPressed()
	{
		LCD.drawString("Pressed " + (touch1.isPressed() || touch2.isPressed()), 0, 1);
		return touch1.isPressed() || touch2.isPressed();
	}

	public int getDistance()
	{
		return distance;
	}
}


